/* gm_sim_physics.c -- Física do simulador
 * 
 * Copyright (C) 2008, Gustavo Campos Martins <>
 * GEAR - Grupo de Estudos Avançados em Robótica
 * <http://www.sel.eesc.usp.br/gear>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */


#include <math.h>
#include <pthread.h>
#include <semaphore.h>

#include "gm_sim.h"
#include "gm_sim_utils.h"
#include "gm_sim_physics.h"


static void * gm_sim_update( gm_sim_t * self );


/** Inicializa a física do simulador.
 *
 * @param *self Estrutura que contém os dados do simulador.
 */
void
gm_sim_phys_init( gm_sim_t * self )
{
	/* inicia o espaço */
	self->space = cpSpaceNew();
	self->space->gravity = cpv(0.0f, 0.0f);
	
	/* Otimizações */
	/* cpSpaceResizeStaticHash(self->space, obj_num, 2000); */
	/* cpSpaceResizeActiveHash(self->space, obj_num, 100); */

	/* inicia os robôs */
	for (int i = 0; i < NUM_ROB; ++i) {
		self->robot[i] = cpBodyNew(INFINITY, INFINITY);
		if (i < 5)
			self->robot[i]->p = cpv(150.0f + 80*i, 700.0f);
		else
			self->robot[i]->p = cpv(430.0f + 80*i, 700.0f);
		cpSpaceAddBody(self->space, self->robot[i]);
		self->robot[i]->a = PI/2;
	}

	cpVect a, b;
	cpShape *seg;
	for (int j = 0; j < NUM_ROB; ++j) {
		for (int i=35; i<325; i++) {
			float alfa = i*DEG2RAD;
			a = cpv(cos(alfa)*ROBOT_RADIUS, sin(alfa)*ROBOT_RADIUS);
			b = cpv(cos(alfa + 1*DEG2RAD)*ROBOT_RADIUS, sin(alfa + 1*DEG2RAD)*ROBOT_RADIUS);

			seg = cpSegmentShapeNew(self->robot[j], a, b, 0.0f);
			seg->e = 0.4;
			seg->u = 100.0;
			cpSpaceAddShape(self->space,seg);
		}
		a = cpv(cos(35*DEG2RAD)*ROBOT_RADIUS, sin(35*DEG2RAD)*ROBOT_RADIUS);
		seg = cpSegmentShapeNew(self->robot[j], b, a, 0.0f);
		seg->e = 0.4;
		seg->u = 100.0;
		cpSpaceAddShape(self->space,seg);
	}

	/* inicia a bola */
	self->ball = cpBodyNew(10.0f, 0.1f);
	self->ball->p = cpv(500.0f, 480.0f);
	self->ball->v = cpv(0.0f, 0.0f);

	cpShape *bShape = cpCircleShapeNew(self->ball, BALL_RADIUS, cpv(0.0f, 0.0f));
	bShape->e = 0.4;
	bShape->u = 100.0;
	cpSpaceAddBody(self->space, self->ball);
	cpSpaceAddShape(self->space, bShape);

	/* inicia as "laterais" do campo */
	cpBody *staticBody = cpBodyNew(INFINITY, INFINITY);
	seg = cpSegmentShapeNew(staticBody, FIELD_BL, FIELD_BR, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_BR, GOALOPP_B, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOPP_B, cpv(WORLD_BR.x, GOALOPP_B.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BR.x, GOALOPP_B.y), cpv(WORLD_BR.x, GOALOPP_U.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BR.x, GOALOPP_U.y), GOALOPP_U, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOPP_U, FIELD_UR, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_UR, FIELD_UL, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, FIELD_UL, GOALOUR_U, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOUR_U, cpv(WORLD_BL.x, GOALOUR_U.y) , 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BL.x, GOALOUR_U.y), cpv(WORLD_BL.x, GOALOUR_B.y), 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, cpv(WORLD_BL.x, GOALOUR_B.y), GOALOUR_B, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);
	seg = cpSegmentShapeNew(staticBody, GOALOUR_B, FIELD_BL, 0.0);
	seg->e = 0.4; seg->u = 100.0;
	cpSpaceAddStaticShape(self->space, seg);

	pthread_t p;
	pthread_create(&p, 0, (void *) gm_sim_update, (void *) self);
	pthread_join(p, 0);
}

/** Thread que controla a física.
 *
 * @param *self Dados do simulador
 */
static void * 
gm_sim_update( gm_sim_t * self )
{
	int count = 10;
	float old_ball_posx = 0;

	//while ( !self->physics_quit )
	{
		/* quanto maior for o steps, mais precisos são os cálculos. */
		int steps = 1;
		cpFloat dt = 1.0/60.0/(cpFloat)steps;

		/*<região crítica>*/
		sem_trywait(&self->change);

		if (count == 10) {
	
			count = 0;


			/* Se a bola foi chutada */
			for (int i = 0; i < NUM_ROB; ++i) {
				if (self->kick[i]) {
					float dist = sqrt( pow(self->ball->p.x - (self->robot[i]->p.x + 
						               cos(self->robot[i]->a)*ROBOT_RADIUS), 2) +
							   pow(self->ball->p.y - (self->robot[i]->p.y +
							       sin(self->robot[i]->a)*ROBOT_RADIUS), 2) );

					if (dist < 15.0)
						cpBodyApplyImpulse(self->ball, 
							   	   cpvmult(cpvsub(self->ball->p, 
								   		  self->robot[i]->p),
										  15.0),
							   	   cpvzero);
					self->kick[i] = 0;
				}
			}

			/* atrito da bola com o chão */
			/* ball->v = cpvmult(ball->v, 0.995); */

			//conta os gols
			if (self->ball->p.x < FIELD_BL.x) {
				if (old_ball_posx > FIELD_BL.x)
					self->placar_a++;
			}	
			else if (self->ball->p.x > FIELD_BR.x) {
				if (old_ball_posx < FIELD_BR.x)
					self->placar_b++;
			}
			old_ball_posx = self->ball->p.x;

		}

		//atualiza o Chipmunk
		cpSpaceStep(self->space, dt);

		sem_post(&self->change);
		/*</região crítica>*/

		count++;
	}
}
